using System;
using System.Collections;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Text.RegularExpressions;
using System.Net;
using System.Net.Sockets;
using System.Windows.Forms;
using System.Diagnostics;
using System.Threading;
using JoystickInterface;

namespace tankClient
{
    public partial class Form1 : Form
    {
        private JoystickInterface.Joystick jst;
        public static ManualResetEvent clientConnected = new ManualResetEvent(false);

        private int pJoystickAxis;
        private Joystick joystick;


        public Form1()
        {
            InitializeComponent();
        }

        bool gotCapabilities = false;

        Socket s = null;
        IPEndPoint serverEndPoint;
        IPAddress serverAddress = null;
        int conPort = 2200;
        Byte[] bytesReceived = new Byte[1];
        


        Int32 port = 2200;
        string server = "localhost";
        TcpClient client;
        Byte[] data,dataOut;
        NetworkStream stream;
        Thread readerThread,parseInDataThread,joystickReaderThread;
        Stack testCmdStack = new Stack();
        Hashtable inData = new Hashtable();
        Hashtable inDataKeys = new Hashtable();
        //int trackvalue = 0;



        private void button1_Click(object sender, EventArgs e)
        {
            port = Int32.Parse(textPort.Text);
            server = textHost.Text;

            try
            {
                // Create a TcpClient.
                // Note, for this client to work you need to have a TcpServer 
                // connected to the same address as specified by the server, port
                // combination.
                

                //if (client==null)
                //{
                try
                {
                    client = new TcpClient(server, port);
                    stream = client.GetStream();
                }
                catch
                {
                }
                //}
                

                // Translate the passed message into ASCII and store it as a Byte array.
                //data = System.Text.Encoding.ASCII.GetBytes(message);

                // Get a client stream for reading and writing.
                //  Stream stream = client.GetStream();

                

                // Send the message to the connected TcpServer. 
                //stream.Write(data, 0, data.Length);
                

                //Console.WriteLine("Sent: {0}", message);


                data = System.Text.Encoding.ASCII.GetBytes("");

                // Get a client stream for reading and writing.
                //  Stream stream = client.GetStream();

                readerThread = new Thread(new ThreadStart(this.sensorreader));
                readerThread.Name = "SensorReaderThread";
                readerThread.IsBackground = true;
                readerThread.Start();

           }
            catch (ArgumentNullException err)
            {
                Console.WriteLine("ArgumentNullException: {0}", err);
            }
            catch (SocketException err)
            {
                Console.WriteLine("SocketException: {0}", err);
            }


        }

        private void button3_Click(object sender, EventArgs e)
        {
            stream.Close();
            client.Close();

        }
        int aByte;

        string command = "";
        string textbuffer,textbuffer1;
        
        String responseData;
        bool connected = false;
        bool oldramp = false;
        String[] list = new string[255];
        String[] commandlist = new string[255];
        string[] keyval = new string[2];

        Regex regFb, regCapMotorFb, regCapSensorFb; //= new Regex("(^.+)=(\\d+|-\\d+)", RegexOptions.Compiled); 
        Match matFb,capMotorFb,capSensorFb;

        public bool parseFeedback(string fbA)
        {
            bool result = false;
            string[] lines = fbA.Split('\n');
            foreach (string fb in lines)
            {
                //Debug.WriteLine("Scanning line \"" + fb + "\"");
                regCapSensorFb = new Regex("Sensor: (\\w+)=\\((\\d+|-\\d+) - (\\d+|-\\d+)\\) (\\d+)", RegexOptions.Compiled); // 1=name, 2=min, 3=max, 4=initial
                capSensorFb = regCapSensorFb.Match(fb);
                regCapMotorFb = new Regex("Motor: (.+)=\\((\\d+|-\\d+) - (\\d+|-\\d+)\\) (\\d+)", RegexOptions.Compiled); // 1=name, 2=min, 3=max, 4=initial
                capMotorFb = regCapMotorFb.Match(fb);
                regFb = new Regex("(^.+)=(\\d+|-\\d+)", RegexOptions.Compiled);
                matFb = regFb.Match(fb);
                

                if (capSensorFb.Success)  // CAPABILITIES Sensors
                {
                    Debug.WriteLine("Found SENSOR capabilities");
                    string key = capSensorFb.Result("$1");
                    string valMin = capSensorFb.Result("$2");
                    string valMax = capSensorFb.Result("$3");
                    string valInitial = capSensorFb.Result("$4");

                    inData[key] = Int32.Parse(valInitial);
                    gotCapabilities = true;

                    result = true;
                }
                if (capMotorFb.Success)  // CAPABILITIES Motors
                {
                    Debug.WriteLine("Found MOTOR capabilities");
                    string key = capMotorFb.Result("$1");
                    string valMin = capMotorFb.Result("$2");
                    string valMax = capMotorFb.Result("$3");
                    string valInitial = capMotorFb.Result("$4");
                    gotCapabilities = true;
                    inData["" + key] = 0; //Int32.Parse(valInitial);
                    Debug.WriteLine("MOTOR KEY: " + key);
                    result = true;
                }
                if (matFb.Success) // SENSOR/MOTOR DATA
                {
                    //Debug.WriteLine("Fannt: " + matFb.ToString());
                    //Debug.WriteLine("Fannt: " + matFb.Result("\"$1\" = \"$2\""));
                    string key = matFb.Result("$1");
                    string val = matFb.Result("$2");

                    inData[key] = Int32.Parse(val);
                    result = true;

                }
            }
            return result;
        }

        bool joystickConnected = false;
        //private Joystick joystick;
        public bool attackJoystick()
        {
            joystick = new Joystick(this.Handle);

            string[] sticks = joystick.FindJoysticks();
//            joystick = Joystick;

            try
            {
                if (joystick.AcquireJoystick(sticks[0]))
                {
                    joystick.UpdateStatus();
                    joystickConnected = true;
                    Debug.WriteLine("Found a joystick");
                    return true;
                }
            }
            catch
            {
                joystickConnected = false;
            }
            //pJoystickAxis = JoystickAxis;
            joystickConnected = false;
            Debug.WriteLine("Did Not find a joystick");
            return false;

        }

        public void joystickReader()
        {
            int yaxis=0;
            int xaxis = 0;
            int youtt = 0;
            int xoutt = 0;
            while (true)
            {
                joystick.UpdateStatus();
                //if (yaxis != joystick.AxisC)
                //{
                
                    xaxis = joystick.AxisC;
                    yaxis = joystick.AxisD;

                    //Debug.WriteLine("Yaxis: " + yaxis.ToString());
                    youtt = ((yaxis*200 / 65535)-100)*(-1);
                    xoutt = ((xaxis * 200 / 65535) - 100);
                    //Debug.WriteLine("Xaxis: " + xoutt.ToString());
                    
                //}

                Thread.Sleep(1);
            }
        }

        public void sensorreader()
        {
            textbuffer1 = "";
            connected = true;

            
            while (connected)
            {
                data = new Byte[255];
                try
                {
                    Int32 bytes = stream.Read(data, 0, data.Length);
                    responseData = System.Text.Encoding.ASCII.GetString(data, 0, bytes);
                    
                    //responseData = responseData.TrimEnd('\n');
                    //responseData = responseData.TrimEnd('\r');
                    responseData = responseData.Replace("\n\n","\n");
                    responseData = responseData.Replace("\r","");
                    responseData = responseData.TrimEnd('\n');
                    responseData = responseData.TrimEnd('\r');
                    
                    //Debug.WriteLine("responsedata: \"" + responseData + "\"");
                    
                    //if (responseData=="\n")
                    //{
                    if (!parseFeedback(responseData))
                    {
                        Debug.WriteLine("Unparsed: " + responseData);
                        //textBox2.Text += responseData;
                    }
                    //}
                    
                }
                catch
                {
                    connected = false;
                    
                }
                Thread.Sleep(1);
            }

        }



        private void timer1_Tick(object sender, EventArgs e)
        {
            // M1
            
            int m1h = Math.Abs(Motor1sp)*(m2p.Height/100)/2;
            int m2h = Math.Abs(Motor2sp)*(m2p.Height/100)/2; 
            if (Motor1sp >= 0)
            {
                m1sp.BackColor = Color.Blue;
                m1sp.Top = motorstartpos - m1h;
                m1sp.Height = m1h;
            }
            else
            {
                m1sp.BackColor = Color.Red;
                m1sp.Height = m1h;
                m1sp.Top = motorstartpos;
            }
            //M2
            if (Motor2sp >= 0)
            {
                m2sp.BackColor = Color.Blue;
                m2sp.Top = motorstartpos - m2h;
                m2sp.Height = m2h;
            }
            else
            {
                m2sp.BackColor = Color.Red;
                m2sp.Height = m2h;
                m2sp.Top = motorstartpos;
            }
            
            //trackBar3.Value = Motor1sp;
            //trackBar4.Value = Motor2sp;

            trackBar4.Value = sim;
            
            if (connected)
            {
                trackBar1.Enabled = true;
                trackBar2.Enabled = true;
                toolStripStatusLabel1.Text = "Connected";
                //textBox3.Enabled = true;
                button2.Enabled = true;
            }
            else
            {
                trackBar1.Enabled = false;
                trackBar2.Enabled = false;
                //textBox3.Enabled = false;
                button2.Enabled = false;
                toolStripStatusLabel1.Text = "Disconnected";
            }
            
           
        }

        int Motor1sp = 0;
        int Motor2sp = 0;
        int sim = 0;
        

        public void parseInData()
        {
            while (true)
            {
             //   Debug.WriteLine("bah");
                //m2sp.Height = (int)inData["M2.sp"];
                if (inData.Count > 0 && gotCapabilities)
                {
                    //inData["M1.sp"] = trackvalue;
                    //inData["M2.sp"] = trackvalue;
                    try
                    {
                        Motor1sp = (int) inData["M1.sp"];
                        Motor2sp = (int) inData["M2.sp"];
                        
                            //sim = (int)inData["sim"];
                        }
                        catch {}

                    //Motor1sp = trackvalue;
                    //Motor2sp = trackvalue;
                   
                }
                Thread.Sleep(1);
            }

        }

        private void button2_Click(object sender, EventArgs e)
        {
            Byte[] thisDataOut = new Byte[256];
            //dataOut = new Byte[256];
            thisDataOut = System.Text.Encoding.ASCII.GetBytes(textBox3.Text + "\r\n");
            stream.Write(thisDataOut, 0, thisDataOut.Length);
        }
        int oldSpeed = 0;
        int oldDir = 0;
        private void trackBar2_Scroll(object sender, EventArgs e)
        {
            //trackvalue = trackBar2.Value;
            

        }

        private void trackBar1_Scroll(object sender, EventArgs e)
        {
           
        }

        string feedback = "";
        private void timer2_Tick(object sender, EventArgs e)
        {
            /*
            if (testCmdStack.Count>0)
            {
                string cmd = (string) testCmdStack.Pop();
               // textBox2.Text += cmd;
                textBox1.Text += cmd;
                textBox1.SelectionStart = textBox1.Text.Length;
               // textbuffer1 = "";
                textBox1.ScrollToCaret();

            }
             */

        }
        
        int motorstartpos = 0;
        private void Form1_Load(object sender, EventArgs e)
        {
            motorstartpos = m2p.Height / 2;
            m2horiz.Height = 1;
            m2horiz.Width = m2p.Width;
            m2horiz.Top = motorstartpos;
            //inData["M2.sp"] = 0;
            //inData["M1.sp"] = 0;
            //inData["sim"] = 0;
            checkBox1.Checked=true;
            oldramp=true;
            parseInDataThread = new Thread(new ThreadStart(this.parseInData));
            parseInDataThread.IsBackground = true;
            parseInDataThread.Start();


            joystickReaderThread = new Thread(new ThreadStart(this.joystickReader));
            joystickReaderThread.IsBackground = true;

            if (attackJoystick())
            {
                //joystickReaderThread.Start();
                joystickTimer.Enabled = true;
            }

          


        }

        private void trackBar2_MouseUp(object sender, MouseEventArgs e)
        {
            
            if (Math.Abs(trackBar2.Value) < 25)
            {

                trackBar2.Value = 0;

                

            }
             
        }

        private void trackBar1_MouseUp(object sender, MouseEventArgs e)
        {
            if (Math.Abs(trackBar1.Value) < 25)
            {
                trackBar1.Value = 0;
            }
        }
        string tmpcmd = "";

        private bool inRange(int value1, int value2, int range, int deadband)
        {
            // deadband av range:           
            decimal deadprc;
            decimal rangemax;
            rangemax = (decimal)range/2;
            deadprc =  (decimal)deadband/100;     // with deadband 50, should equal 0.5
            decimal deadReal = range * deadprc;   // with range 200 and deadband 50, should equal 100
            decimal diff = Math.Abs(value1 - value2);
            //Debug.WriteLine("Allowed range: " + deadReal.ToString() + " , Diff: " + diff.ToString());
            
            if (diff <= deadReal)
            {
                if (diff > 0) // have a small diff
                {
                    // one of the values is equal to the max or min, force setting of the value
                    if ((Math.Abs(value1) == (int)rangemax) || (Math.Abs(value2) == (int)rangemax))
                    {
                        return false;
                    }
                    else
                    {
                        return true;
                    }

                }
                else          // Values are equal
                {
                    return true;
                }
            }
            else
            {
                return false;
            }

            //int lower = (range * (1 - deadband / 100));
            //int upper = (range * (1 + deadband / 100));

            //Debug.WriteLine("Lower: " + lower.ToString() + ", Upper: " + upper.ToString());
            //if (value2 > lower && value2 < upper)
            //{
            //    return true;
            //}
            //else
            //{
            //    return false;
            //}

        }

        private void sendSpeedTimer_Tick(object sender, EventArgs e)
        {
            int deadband = 2; // Minimum change in trackbars (in %)
            if (connected)
            {
                dataOut = new Byte[256];
                tmpcmd = "";
                //if(oldDir!=trackBar1.Value)
                //if ((oldDir <= trackBar1.Value * (1-deadband/100)) || (oldDir >= trackBar1.Value * (1 + deadband / 100)))
                if(!inRange(trackBar1.Value,oldDir,200,deadband))
                {
                    oldDir = trackBar1.Value;
                    if (Math.Abs(oldDir)<5)          {oldDir=0;}
                    tmpcmd += "dir=" + oldDir.ToString();
                    tmpcmd += "\r\n";
                }
                //if(oldSpeed!=trackBar2.Value)
                if (!inRange(trackBar2.Value, oldSpeed, 200, deadband))
                {
                    oldSpeed = trackBar2.Value;
                    if (Math.Abs(oldSpeed) < 5)     {oldSpeed = 0; }
                    tmpcmd += "speed=" + oldSpeed.ToString();
                    tmpcmd += "\r\n";
                }
                if(oldramp!=checkBox1.Checked)
                {
                    oldramp = checkBox1.Checked;
                    if(oldramp)
                    {
                        tmpcmd += "ramp=ON";
                    }
                    else
                    {
                        tmpcmd += "ramp=OFF";
                    }
                    tmpcmd += "\r\n";
                }
                if(tmpcmd!="")
                {
                dataOut = System.Text.Encoding.ASCII.GetBytes(tmpcmd);
                stream.Write(dataOut, 0, dataOut.Length);
                }
            }
                      
        }

        private void joystickTimer_Tick(object sender, EventArgs e)
        {
            int yaxis = 0;
            int xaxis = 0;
            int youtt = 0;
            int xoutt = 0;
            
                joystick.UpdateStatus();
                //if (yaxis != joystick.AxisC)
                //{

                xaxis = joystick.AxisC;
                yaxis = joystick.AxisD;

                //Debug.WriteLine("Yaxis: " + yaxis.ToString());
                youtt = ((yaxis * 200 / 65535) - 100) * (-1);
                xoutt = ((xaxis * 200 / 65535) - 100);
                //Debug.WriteLine("Xaxis: " + xoutt.ToString());
                trackBar2.Value = youtt;
                trackBar1.Value = xoutt;
                //}

            
        }

        private void textBox3_TextChanged(object sender, EventArgs e)
        {

        }

    }
}